using System;
using System.Collections.Generic;
using System.ComponentModel;
using Microsoft.Ccr.Core;
using Microsoft.Dss.Core.Attributes;
using Microsoft.Dss.ServiceModel.Dssp;
using Microsoft.Dss.ServiceModel.DsspServiceBase;
using W3C.Soap;
using submgr = Microsoft.Dss.Services.SubscriptionManager;
using drive = Microsoft.Robotics.Services.Drive.Proxy;
using SRV1.Properties;

namespace SRV1.Drive
{
    [Contract(Contract.Identifier)]
    [DisplayName("SRV-1 Drive")]
    [Description("New Drive service for SRV-1")]
    class DriveService : DsspServiceBase
    {
        /// <summary>
        /// Service state
        /// </summary>
        [ServiceState]
        DriveState _state = new DriveState();

        /// <summary>
        /// Main service port
        /// </summary>
        [ServicePort("/Drive", AllowMultipleInstances = true)]
        DriveOperations _mainPort = new DriveOperations();

        private static AForgeNetController.AForgeNetController _srv1 = new AForgeNetController.AForgeNetController();

        [SubscriptionManagerPartner]
        submgr.SubscriptionManagerPort _submgrPort = new submgr.SubscriptionManagerPort();

        /// <summary>
        /// Alternate service port
        /// </summary>
        [AlternateServicePort(AlternateContract = drive.Contract.Identifier)]
        drive.DriveOperations _driveDifferentialTwoWheelPort = new drive.DriveOperations();

        /// <summary>
        /// Service constructor
        /// </summary>
        public DriveService(DsspServiceCreationPort creationPort)
            : base(creationPort)
        {
            if (_srv1.IsConnected != true)
            {
                _srv1.Connect(Settings.Default.IpAddress);
            }
        }

        /// <summary>
        /// Service start
        /// </summary>
        protected override void Start()
        {

            // 
            // Add service specific initialization here
            // 

            base.Start();
        }

        /// <summary>
        /// Handles Subscribe messages
        /// </summary>
        /// <param name="subscribe">the subscribe request</param>
        //[ServiceHandler]
        //public void SubscribeHandler(Subscribe subscribe)
        //{
        //    SubscribeHelper(_submgrPort, subscribe.Body, subscribe.ResponsePort);
        //}

        /// <summary>
        /// Handles Get requests on alternate port DriveDifferentialTwoWheel
        /// </summary>
        /// <param name="get">request message</param>
        [ServiceHandler(PortFieldName = "_driveDifferentialTwoWheelPort")]
        public virtual IEnumerator<ITask> DriveDifferentialTwoWheelGetHandler(drive.Get get)
        {
            get.ResponsePort.Post((drive.DriveDifferentialTwoWheelState)_state.Clone());

            yield break;
        }

        /// <summary>
        /// Handles HttpGet requests on alternate port DriveDifferentialTwoWheel
        /// </summary>
        /// <param name="httpget">request message</param>
        [ServiceHandler(PortFieldName = "_driveDifferentialTwoWheelPort")]
        public void DriveDifferentialTwoWheelHttpGetHandler(Microsoft.Dss.Core.DsspHttp.HttpGet httpget)
        {
            throw new NotImplementedException();
        }

        /// <summary>
        /// Handles HttpPost requests on alternate port DriveDifferentialTwoWheel
        /// </summary>
        /// <param name="httppost">request message</param>
        [ServiceHandler(PortFieldName = "_driveDifferentialTwoWheelPort")]
        public void DriveDifferentialTwoWheelHttpPostHandler(Microsoft.Dss.Core.DsspHttp.HttpPost httppost)
        {
            throw new NotImplementedException();
        }

        /// <summary>
        /// Handles ReliableSubscribe requests on alternate port DriveDifferentialTwoWheel
        /// </summary>
        /// <param name="reliablesubscribe">request message</param>
        [ServiceHandler(PortFieldName = "_driveDifferentialTwoWheelPort")]
        [ServiceHandler(ServiceHandlerBehavior.Concurrent)]
        public virtual void DriveDifferentialTwoWheelReliableSubscribeHandler(drive.ReliableSubscribe reliablesubscribe)
        {
            LogInfo("Generic Drive Reliable Subscription from: " + reliablesubscribe.Body.Subscriber);
            SubscribeHelper(_submgrPort, reliablesubscribe.Body, reliablesubscribe.ResponsePort);
        }

        /// <summary>
        /// Handles Subscribe requests on alternate port DriveDifferentialTwoWheel
        /// </summary>
        /// <param name="subscribe">request message</param>
        [ServiceHandler(PortFieldName = "_driveDifferentialTwoWheelPort")]
        [ServiceHandler(ServiceHandlerBehavior.Concurrent)]
        public virtual IEnumerator<ITask> DriveDifferentialTwoWheelSubscribeHandler(drive.Subscribe subscribe)
        {
            LogInfo("Generic Drive Subscription from: " + subscribe.Body.Subscriber);
            SubscribeHelper(_submgrPort, subscribe.Body, subscribe.ResponsePort);

            yield break;
        }

        /// <summary>
        /// Handles Update requests on alternate port DriveDifferentialTwoWheel
        /// </summary>
        /// <param name="update">request message</param>
        [ServiceHandler(PortFieldName = "_driveDifferentialTwoWheelPort")]
        public void DriveDifferentialTwoWheelUpdateHandler(drive.Update update)
        {
            throw new NotImplementedException();
        }

        /// <summary>
        /// Handles EnableDrive requests on alternate port DriveDifferentialTwoWheel
        /// </summary>
        /// <param name="enabledrive">request message</param>
        [ServiceHandler(PortFieldName = "_driveDifferentialTwoWheelPort")]
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        public virtual IEnumerator<ITask> DriveDifferentialTwoWheelEnableDriveHandler(drive.EnableDrive enabledrive)
        {
            enabledrive.ResponsePort.Post(DefaultUpdateResponseType.Instance);

            yield break;
        }

        /// <summary>
        /// Handles SetDrivePower requests on alternate port DriveDifferentialTwoWheel
        /// </summary>
        /// <param name="setdrivepower">request message</param>
        [ServiceHandler(PortFieldName = "_driveDifferentialTwoWheelPort")]
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        public virtual IEnumerator<ITask> DriveDifferentialTwoWheelSetDrivePowerHandler(drive.SetDrivePower setdrivepower)
        {
            drive.Update update = new Microsoft.Robotics.Services.Drive.Proxy.Update();

            // Set speed for one second or until another command is received.
            int duration = 500;
            int leftWheelPower = (int)(setdrivepower.Body.LeftWheelPower * 100);
            int rightWheelPower = (int)(setdrivepower.Body.RightWheelPower * 100);

            _srv1.Motor(leftWheelPower, rightWheelPower, duration);

            setdrivepower.ResponsePort.Post(DefaultUpdateResponseType.Instance);

            yield break;
        }

        /// <summary>
        /// Handles SetDriveSpeed requests on alternate port DriveDifferentialTwoWheel
        /// </summary>
        /// <param name="setdrivespeed">request message</param>
        [ServiceHandler(PortFieldName = "_driveDifferentialTwoWheelPort")]
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        public virtual IEnumerator<ITask> DriveDifferentialTwoWheelSetDriveSpeedHandler(drive.SetDriveSpeed setdrivespeed)
        {
            drive.Update update = new Microsoft.Robotics.Services.Drive.Proxy.Update();

            // Set speed for one second or until another command is received.
            int duration = 500;
            int leftWheelPower = (int)(setdrivespeed.Body.LeftWheelSpeed * 100);
            int rightWheelPower = (int)(setdrivespeed.Body.RightWheelSpeed * 100);

            _srv1.Motor(leftWheelPower, rightWheelPower, duration);

            setdrivespeed.ResponsePort.Post(DefaultUpdateResponseType.Instance);

            yield break;
        }

        /// <summary>
        /// Handles RotateDegrees requests on alternate port DriveDifferentialTwoWheel
        /// </summary>
        /// <param name="rotatedegrees">request message</param>
        [ServiceHandler(PortFieldName = "_driveDifferentialTwoWheelPort")]
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        public virtual IEnumerator<ITask> DriveDifferentialTwoWheelRotateDegreesHandler(drive.RotateDegrees rotatedegrees)
        {
            int leftdrivePower = -48;
            int rightdrivePower = 48;

            if (rotatedegrees.Body.Degrees < 0)
            {
                leftdrivePower = leftdrivePower * -1;
                rightdrivePower = rightdrivePower * -1;
            }

            float duration = (float)rotatedegrees.Body.Degrees / (float)180;
            duration = duration * 1000;

            _srv1.Motor(leftdrivePower, rightdrivePower, (int)duration);

            rotatedegrees.ResponsePort.Post(DefaultUpdateResponseType.Instance);

            yield break;
        }

        /// <summary>
        /// Handles DriveDistance requests on alternate port DriveDifferentialTwoWheel
        /// </summary>
        /// <param name="drivedistance">request message</param>
        [ServiceHandler(PortFieldName = "_driveDifferentialTwoWheelPort")]
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        public virtual IEnumerator<ITask> DriveDifferentialTwoWheelDriveDistanceHandler(drive.DriveDistance drivedistance)
        {
            int drivePower = 45;

            double duration = (drivedistance.Body.Distance / .41) * 1000;

            _srv1.Motor(drivePower, drivePower, (int)duration);

            drivedistance.ResponsePort.Post(DefaultUpdateResponseType.Instance);

            yield break;
        }

        /// <summary>
        /// Handles AllStop requests on alternate port DriveDifferentialTwoWheel
        /// </summary>
        /// <param name="allstop">request message</param>
        [ServiceHandler(ServiceHandlerBehavior.Concurrent, PortFieldName = "_driveDifferentialTwoWheelPort")]
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        public virtual IEnumerator<ITask> DriveDifferentialTwoWheelAllStopHandler(drive.AllStop allstop)
        {
            _srv1.StopMotors();

            allstop.ResponsePort.Post(DefaultUpdateResponseType.Instance);

            yield break;
        }
    }
}


